﻿using SoftFloat;
using System.Runtime.CompilerServices;

namespace BepuUtilities
{
    public struct Vector2
    {
        public sfloat X, Y;

        public static Vector2 Zero
        {
            get
            {
                return default;
            }
        }

        public Vector2(sfloat x, sfloat y)
        {
            X = x;
            Y = y;
        }

        public Vector2(sfloat value)
        {
            this = new Vector2(value, value);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static sfloat Dot(Vector2 value1, Vector2 value2)
        {
            return value1.X * value2.X + value1.Y * value2.Y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat Length()
        {
            sfloat x = LengthSquared();
            return libm.Sqrt(x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public readonly sfloat LengthSquared()
        {
            return Dot(this, this);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 Normalize(Vector2 value)
        {
            return value / value.Length();
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 Max(Vector2 value1, Vector2 value2)
        {
            return new Vector2((value1.X > value2.X) ? value1.X : value2.X, (value1.Y > value2.Y) ? value1.Y : value2.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 Min(Vector2 value1, Vector2 value2)
        {
            return new Vector2((value1.X < value2.X) ? value1.X : value2.X, (value1.Y < value2.Y) ? value1.Y : value2.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static sfloat Distance(Vector2 value1, Vector2 value2)
        {
            sfloat x = DistanceSquared(value1, value2);
            return libm.Sqrt(x);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static sfloat DistanceSquared(Vector2 value1, Vector2 value2)
        {
            Vector2 vector = value1 - value2;
            return Dot(vector, vector);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator -(Vector2 value)
        {
            return Zero - value;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator -(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X - right.X, left.Y - right.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator /(Vector2 value1, sfloat value2)
        {
            return value1 / new Vector2(value2);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator /(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X / right.X, left.Y / right.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator *(Vector2 left, sfloat right)
        {
            return left * new Vector2(right);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator *(sfloat left, Vector2 right)
        {
            return right * left;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator *(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X * right.X, left.Y * right.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static Vector2 operator +(Vector2 left, Vector2 right)
        {
            return new Vector2(left.X + right.X, left.Y + right.Y);
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator ==(Vector2 left, Vector2 right)
        {
            return left.X == right.X && left.Y == right.Y;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static bool operator !=(Vector2 left, Vector2 right)
        {
            return !(left == right);
        }

        public static implicit operator Vector2(System.Numerics.Vector2 a)
        {
            return new Vector2(a.X, a.Y);
        }

        public static implicit operator System.Numerics.Vector2(Vector2 a)
        {
            return new System.Numerics.Vector2(a.X, a.Y);
        }

        public override string ToString()
        {
            return $"({X.ToString("G")}, {Y.ToString("G")})";
        }
    }
}
